////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//	MathMatrix4x4.cpp
//  MathLib
//
//  Created by Gregory Maks on 8/14/09.
//  Copyright 2010 Immersion Digital. All rights reserved.
//
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include "Math.h"
#include <stdio.h>
#include <string.h>
#include <math.h>



//================== Matrix4x4 =====================
Matrix4x4::Matrix4x4 ()
{
	InitWithIdentity();
}

Matrix4x4::Matrix4x4 (floatType* array, MatrixArrayType arrType)
{
	InitWithData(array, arrType);
}

void Matrix4x4::ctor ()
{
	memset(n, 0, sizeof(floatType)*16);
}


// matrix creation methods
void Matrix4x4::InitWithIdentity ()
{
	ctor();
	n[0][0] = n[1][1] = n[2][2] = n[3][3] = 1.0;
}

void Matrix4x4::InitWithData (floatType *array, MatrixArrayType arrType)
{
	if (arrType == MatrixArrayType_ColumnMajor) 
	{
		n[0][0] = array[0]; n[1][0] = array[4]; n[2][0] = array[8];  n[3][0] = array[12];
		n[0][1] = array[1]; n[1][1] = array[5]; n[2][1] = array[9];  n[3][1] = array[13];
		n[0][2] = array[2]; n[1][2] = array[6]; n[2][2] = array[10]; n[3][2] = array[14];
		n[0][3] = array[3]; n[1][3] = array[7]; n[2][3] = array[11]; n[3][3] = array[15];
	}
	else 
	{
		n[0][0] = array[0];  n[1][0] = array[1];  n[2][0] = array[2];  n[3][0] = array[3];
		n[0][1] = array[4];  n[1][1] = array[5];  n[2][1] = array[6];  n[3][1] = array[7];
		n[0][2] = array[8];  n[1][2] = array[9];  n[2][2] = array[10]; n[3][2] = array[11];
		n[0][3] = array[12]; n[1][3] = array[13]; n[2][3] = array[14]; n[3][3] = array[15];
	}
}

void Matrix4x4::InitWithRotation (AxisType axis, floatType angle)
{
	float cosA = cosf(angle);
	float sinA = sinf(angle);
	
	switch (axis)
	{
		case AxisType_X:
			n[0][0] = 1; 	n[1][0] = 0; 	n[2][0] = 0; 	n[3][0] = 0;
			n[0][1] = 0; 	n[1][1] = cosA;	n[2][1] = -sinA;n[3][1] = 0;
			n[0][2] = 0; 	n[1][2] = sinA;	n[2][2] = cosA; n[3][2] = 0;
			n[0][3] = 0; 	n[1][3] = 0; 	n[2][3] = 0; 	n[3][3] = 1;
			break;
			
		case AxisType_Y:
			n[0][0] = cosA; n[1][0] = 0; 	n[2][0] = sinA;	n[3][0] = 0;
			n[0][1] = 0; 	n[1][1] = 1; 	n[2][1] = 0; 	n[3][1] = 0;
			n[0][2] = -sinA;n[1][2] = 0; 	n[2][2] = cosA;	n[3][2] = 0;
			n[0][3] = 0; 	n[1][3] = 0; 	n[2][3] = 0; 	n[3][3] = 1;
			break;
			
		case AxisType_Z:
			n[0][0] = cosA; n[1][0] = -sinA; 	n[2][0] = 0; n[3][0] = 0;
			n[0][1] = sinA; n[1][1] = cosA; 	n[2][1] = 0; n[3][1] = 0;
			n[0][2] = 0; 	n[1][2] = 0; 		n[2][2] = 1; n[3][2] = 0;
			n[0][3] = 0; 	n[1][3] = 0; 		n[2][3] = 0; n[3][3] = 1;
			break;
	}
}

void Matrix4x4::InitWithRotation (Vector4 &a, floatType angle)
{
	float cosA = cosf(angle);
	float sinA = sinf(angle);
	float c = 1 - cosA;	
	
	a.Normalize();
	float ax2 = a.x * a.x;
	float ay2 = a.y * a.y;
	float az2 = a.z * a.z;	
	
	// Formulae taken from http://en.wikipedia.org/wiki/Rotation_matrix
	
	n[0][0] = ax2 + (1 - ax2) * cosA;		n[1][0] = a.x * a.y * c - a.z * sinA; 	n[2][0] = a.x * a.z * c + a.y * sinA;	n[3][0] = 0;
	n[0][1] = a.x * a.y * c + a.z * sinA;	n[1][1] = ay2 + (1 - ay2) * cosA;		n[2][1] = a.y * a.z * c - a.x * sinA;	n[3][1] = 0;
	n[0][2] = a.x * a.z * c - a.y * sinA;	n[1][2] = a.y * a.z * c + a.x * sinA; 	n[2][2] = az2 + (1 - az2) * cosA;		n[3][2] = 0;
	n[0][3] = 0;							n[1][3] = 0;							n[2][3] = 0;							n[3][3] = 1;
}

void Matrix4x4::InitWithRotationYawPitchRoll (floatType yawAngle, floatType pitchAngle, floatType rollAngle)
{
	floatType cy = cos(yawAngle), sy = sin(yawAngle);
	floatType cp = cos(pitchAngle), sp = sin(pitchAngle);
	floatType cr = cos(rollAngle), sr = sin(rollAngle);
	
	// Formulae taken from http://en.wikipedia.org/wiki/Rotation_matrix
	
	n[0][0] = cy*cp;	n[1][0] = (cy*sp*sr - sy*cr);	n[2][0] = (cy*sp*cr + sy*sr);	n[3][0] = 0;
	n[0][1] = sy*sp;	n[1][1] = (sy*sp*sr + cy*cr);	n[2][1] = (sy*sp*cr - cy*sr);	n[3][1] = 0;
	n[0][2] = -sp;		n[1][2] = cp*sr;				n[2][2] = cp*cr;				n[3][2] = 0;
	n[0][3] = 0;		n[1][3] = 0;					n[2][3] = 0;					n[3][3] = 1;
}

void Matrix4x4::InitWithTranslation (floatType x, floatType y, floatType z)
{
	InitWithIdentity();
	
	n[3][0] = x;
	n[3][1] = y;
	n[3][2] = z;
}

void Matrix4x4::InitWithScaling (floatType sizeX, floatType sizeY, floatType sizeZ)
{
	InitWithIdentity();
	
	n[0][0] = sizeX;
	n[1][1] = sizeY;
	n[2][2] = sizeZ;
}

void Matrix4x4::InitWithQuaternion (Quaternion &quat)
{
	InitWithIdentity();
	
	floatType ww = quat.w*quat.w;
	floatType xx = quat.x*quat.x;
	floatType yy = quat.y*quat.y;
	floatType zz = quat.z*quat.z;
	
	n[0][0] = ww + xx - yy - zz;					n[1][0] = 2*quat.x*quat.y - 2*quat.w*quat.z;	n[2][0] = 2*quat.x*quat.z + 2*quat.w*quat.y;
	n[0][1] = 2*quat.x*quat.y + 2*quat.w*quat.z;	n[1][1] = ww - xx + yy - zz;					n[2][1] = 2*quat.y*quat.z - 2*quat.w*quat.x;
	n[0][2] = 2*quat.x*quat.z - 2*quat.w*quat.y;	n[1][2] = 2*quat.y*quat.z - 2*quat.w*quat.x;	n[2][2] = ww - xx - yy + zz;
}

void Matrix4x4::InitWithUnitQuaternion (Quaternion &quat)
{
	InitWithIdentity();
	
	floatType xx = quat.x*quat.x;
	floatType yy = quat.y*quat.y;
	floatType zz = quat.z*quat.z;
	
	n[0][0] = 1 - 2*yy - 2*zz;						n[1][0] = 2*quat.x*quat.y - 2*quat.w*quat.z;	n[2][0] = 2*quat.x*quat.z + 2*quat.w*quat.y;
	n[0][1] = 2*quat.x*quat.y + 2*quat.w*quat.z;	n[1][1] = 1 - 2*xx - 2*zz;						n[2][1] = 2*quat.y*quat.z - 2*quat.w*quat.x;
	n[0][2] = 2*quat.x*quat.z - 2*quat.w*quat.y;	n[1][2] = 2*quat.y*quat.z - 2*quat.w*quat.x;	n[2][2] = 1 - 2*xx - 2*yy;	
}

void Matrix4x4::InitWithLookAt (floatType eyex, floatType eyey, floatType eyez, 
								floatType centerx, floatType centery, floatType centerz, 
								floatType upx, floatType upy, floatType upz)
{
	Vector4 forward(centerx - eyex,
					centery - eyey,
					centerz - eyez);	
	Vector4 up(upx, upy, upz);
	Vector4 side;
	
	forward.Normalize();
	
    // Side = forward x up
	side = forward.CrossProduct(up);
	side.Normalize();
	
    // Recompute up as: up = side x forward
	up = side.CrossProduct(forward);
	
	// Fillup the matrix
	InitWithIdentity();
	n[0][0] = side.x;
    n[1][0] = side.y;
    n[2][0] = side.z;
	
    n[0][1] = up.x;
    n[1][1] = up.y;
    n[2][1] = up.z;
	
    n[0][2] = -forward.x;
    n[1][2] = -forward.y;
    n[2][2] = -forward.z;
	
	Matrix4x4 translationMatrix;
	translationMatrix.InitWithTranslation(-eyex, -eyey, -eyez);
	Multiply(translationMatrix);
}

void Matrix4x4::InitWithPerspective (floatType fovy, floatType aspect, floatType zNear, floatType zFar)
{
    floatType sine, cotangent, deltaZ;
    floatType radians = fovy / 2 * (M_PI/180.0);
	
    deltaZ = zFar - zNear;
    sine = sinf(radians);
    if ((deltaZ == 0) || (sine == 0) || (aspect == 0)) {
		return;
    }
    cotangent = cosf(radians) / sine;
	
    InitWithIdentity();
    n[0][0] = cotangent / aspect;
    n[1][1] = cotangent;
    n[2][2] = -(zFar + zNear) / deltaZ;
    n[2][3] = -1;
    n[3][2] = -2 * zNear * zFar / deltaZ;
    n[3][3] = 0;
}

void Matrix4x4::InitWithOrtho (floatType leftPlane, floatType rightPlane, 
							   floatType bottomPlane, floatType topPlane,
							   floatType nearPlane, floatType farPlane)
{
	floatType tx = - (rightPlane + leftPlane) / (rightPlane - leftPlane);
	floatType ty = - (topPlane + bottomPlane) / (topPlane - bottomPlane);
	floatType tz = - (farPlane + nearPlane) / (farPlane - nearPlane);
	
    InitWithIdentity();
	n[0][0] = 2 / (rightPlane - leftPlane);
	n[1][1] = 2 / (topPlane - bottomPlane);
	n[2][2] = -2 / (farPlane - nearPlane);
	n[3][3] = 1;
	
	n[3][0] = tx;
	n[3][1] = ty;
	n[3][2] = tz;
}

// properties
bool Matrix4x4::IsIdentity ()
{
	return (n[0][0] == 1 && n[0][1] == 0 && n[0][2] == 0 && n[0][3] == 0 &&
			n[1][0] == 0 && n[1][1] == 1 && n[1][2] == 0 && n[1][3] == 0 &&
			n[2][0] == 0 && n[2][1] == 0 && n[2][2] == 1 && n[2][3] == 0 &&
			n[3][0] == 0 && n[3][1] == 0 && n[3][2] == 0 && n[3][3] == 1);
}


// methods
floatType Matrix4x4::Determinant ()
{
	floatType det =  
	n[0][0]*n[1][1]*n[2][2]*n[3][3]
	-n[0][0]*n[1][1]*n[2][3]*n[3][2]
	+n[0][0]*n[1][2]*n[2][3]*n[3][1]
	-n[0][0]*n[1][2]*n[2][1]*n[3][3]
	+n[0][0]*n[1][3]*n[2][1]*n[3][2]
	-n[0][0]*n[1][3]*n[2][2]*n[3][1]
	-n[0][1]*n[1][2]*n[2][3]*n[3][0]
	+n[0][1]*n[1][2]*n[2][0]*n[3][3]
	-n[0][1]*n[1][3]*n[2][0]*n[3][2]
	+n[0][1]*n[1][3]*n[2][2]*n[3][0]
	-n[0][1]*n[1][0]*n[2][2]*n[3][3]
	+n[0][1]*n[1][0]*n[2][3]*n[3][2]
	+n[0][2]*n[1][3]*n[2][0]*n[3][1]
	-n[0][2]*n[1][3]*n[2][1]*n[3][0]
	+n[0][2]*n[1][0]*n[2][1]*n[3][3]
	-n[0][2]*n[1][0]*n[2][3]*n[3][1]
	+n[0][2]*n[1][1]*n[2][3]*n[3][0]
	-n[0][2]*n[1][1]*n[2][0]*n[3][3]
	-n[0][3]*n[1][0]*n[2][1]*n[3][2]
	+n[0][3]*n[1][0]*n[2][2]*n[3][1]
	-n[0][3]*n[1][1]*n[2][2]*n[3][0]
	+n[0][3]*n[1][1]*n[2][0]*n[3][2]
	-n[0][3]*n[1][2]*n[2][0]*n[3][1]
	+n[0][3]*n[1][2]*n[2][1]*n[3][0];
	return det;
}

void Matrix4x4::Inverse ()
{	
	Matrix4x4 destM;	
	
    // get the determinant of a  
    floatType det = 1.0 / Determinant();  
	
    // minor matrix
	floatType *minor = new floatType [9];
	
	// inverting
    for (int j=0; j<4; j++)  
    {  
        for (int i=0; i<4; i++)  
        {  
            // get the co-factor (matrix) of A(j,i)  
            GetMinorMatrix((floatType*)this->n, minor, j, i, 4);  
            destM.n[i][j] = det * CalcCustomDeterminant(minor, 3);  
            if((i+j)%2 == 1)  
                destM.n[i][j] = -destM.n[i][j];
        }  
    }  
	
    // release memory
	delete [] minor;
	
	*this = destM;
}

void Matrix4x4::Transpose ()
{
	floatType fTemp = 0;
	for (int i=0; i<4; i++)
	{
		for (int j=0; j<4; j++)
		{
			fTemp = n[j][i];
			n[j][i] = n[i][j];
			n[i][j] = fTemp;
		}
	}
}

void Matrix4x4::Multiply (Matrix4x4 &mat)
{
	Matrix4x4 temp;
	
	temp.n[0][0] = mat.n[0][0] * n[0][0] + mat.n[0][1] * n[1][0] + mat.n[0][2] * n[2][0] + mat.n[0][3] * n[3][0];
	temp.n[0][1] = mat.n[0][0] * n[0][1] + mat.n[0][1] * n[1][1] + mat.n[0][2] * n[2][1] + mat.n[0][3] * n[3][1];
	temp.n[0][2] = mat.n[0][0] * n[0][2] + mat.n[0][1] * n[1][2] + mat.n[0][2] * n[2][2] + mat.n[0][3] * n[3][2];
	temp.n[0][3] = mat.n[0][0] * n[0][3] + mat.n[0][1] * n[1][3] + mat.n[0][2] * n[2][3] + mat.n[0][3] * n[3][3];
	
	temp.n[1][0] = mat.n[1][0] * n[0][0] + mat.n[1][1] * n[1][0] + mat.n[1][2] * n[2][0] + mat.n[1][3] * n[3][0];
	temp.n[1][1] = mat.n[1][0] * n[0][1] + mat.n[1][1] * n[1][1] + mat.n[1][2] * n[2][1] + mat.n[1][3] * n[3][1];
	temp.n[1][2] = mat.n[1][0] * n[0][2] + mat.n[1][1] * n[1][2] + mat.n[1][2] * n[2][2] + mat.n[1][3] * n[3][2];
	temp.n[1][3] = mat.n[1][0] * n[0][3] + mat.n[1][1] * n[1][3] + mat.n[1][2] * n[2][3] + mat.n[1][3] * n[3][3];
	
	temp.n[2][0] = mat.n[2][0] * n[0][0] + mat.n[2][1] * n[1][0] + mat.n[2][2] * n[2][0] + mat.n[2][3] * n[3][0];
	temp.n[2][1] = mat.n[2][0] * n[0][1] + mat.n[2][1] * n[1][1] + mat.n[2][2] * n[2][1] + mat.n[2][3] * n[3][1];
	temp.n[2][2] = mat.n[2][0] * n[0][2] + mat.n[2][1] * n[1][2] + mat.n[2][2] * n[2][2] + mat.n[2][3] * n[3][2];
	temp.n[2][3] = mat.n[2][0] * n[0][3] + mat.n[2][1] * n[1][3] + mat.n[2][2] * n[2][3] + mat.n[2][3] * n[3][3];
	
	temp.n[3][0] = mat.n[3][0] * n[0][0] + mat.n[3][1] * n[1][0] + mat.n[3][2] * n[2][0] + mat.n[3][3] * n[3][0];
	temp.n[3][1] = mat.n[3][0] * n[0][1] + mat.n[3][1] * n[1][1] + mat.n[3][2] * n[2][1] + mat.n[3][3] * n[3][1];
	temp.n[3][2] = mat.n[3][0] * n[0][2] + mat.n[3][1] * n[1][2] + mat.n[3][2] * n[2][2] + mat.n[3][3] * n[3][2];
	temp.n[3][3] = mat.n[3][0] * n[0][3] + mat.n[3][1] * n[1][3] + mat.n[3][2] * n[2][3] + mat.n[3][3] * n[3][3];
	
	*this = temp;
}

void Matrix4x4::ConvertToColumnMajorArray (float *array)
{
	// This conversion is right because the OpenGL uses 12,13,14 cells as Transformation matrix.
	array[0] = n[0][0]; array[4] = n[1][0]; array[8]  = n[2][0]; array[12] = n[3][0];
	array[1] = n[0][1]; array[5] = n[1][1]; array[9]  = n[2][1]; array[13] = n[3][1];
	array[2] = n[0][2]; array[6] = n[1][2]; array[10] = n[2][2]; array[14] = n[3][2];
	array[3] = n[0][3]; array[7] = n[1][3]; array[11] = n[2][3]; array[15] = n[3][3];
}

void Matrix4x4::ConvertToRowMajorArray (float *array)
{
	array[0] = n[0][0]; array[4] = n[0][1]; array[8] = n[0][2]; array[12] = n[0][3];
	array[1] = n[1][0]; array[5] = n[1][1]; array[9] = n[1][2]; array[13] = n[1][3];
	array[2] = n[2][0]; array[6] = n[2][1]; array[10] = n[2][2]; array[14] = n[2][3];
	array[3] = n[3][0]; array[7] = n[3][1]; array[11] = n[3][2]; array[15] = n[3][3];
}

// debug methods
void Matrix4x4::Printf()
{
	printf("Matrix4x4 -------------\n");
	for (int row=0; row<4; row++)
	{
		printf("%.2f %.2f %.2f %.2f\n", n[0][row], n[1][row], n[2][row], n[3][row]);
	}
	printf("-----------------------\n");
}


// operators
Matrix4x4 Matrix4x4::operator* (Matrix4x4 &mat)
{
	Multiply(mat);
	return *this;
}

Matrix4x4& Matrix4x4::operator*= (Matrix4x4 &mat)
{
	Multiply(mat);
	return *this;
}

bool Matrix4x4::operator== (Matrix4x4 &mat)
{
	bool res = true;
	for (int i=0; i<4; i++) {
		for (int j=0; j<4; j++) {
			if (n[i][j] != mat.n[i][j]) 
			{
				res = false;
				i=4; break;
			}
		}
	}
	return res;
}

bool Matrix4x4::operator!= (Matrix4x4 &mat)
{
	return !((*this) == mat);
}

// private methods
void Matrix4x4::GetMinorMatrix(floatType *src, floatType *dest, int row, int col, int order)
{
    // indicate which col and row is being copied to dest
    int colCount = 0, rowCount = 0;
	
    for (int i=0; i<order; i++)
    {
        if (i != row)
        {
            colCount = 0;
            for (int j=0; j<order; j++)
            {
                // when j is not the element
                if (j != col)
                {
                    dest[rowCount*(order-1)+colCount] = src[i*order+j];
                    colCount++;
                }
            }
            rowCount++;
        }
    }
}

floatType Matrix4x4::CalcCustomDeterminant (floatType *mat, int order)
{
    // order must be >= 0  
    // stop the recursion when matrix is a single element  
    if (order == 1)  
        return mat[0];
	
    // the determinant value  
    floatType det = 0;  
	
    // allocate the cofactor matrix (dynamix 2-dimensional array)
    floatType *minor = new floatType[(order-1)*(order-1)];
	
    for (int i=0; i<order; i++)  
    {  
        // get minor of element (0,i)  
        GetMinorMatrix(mat, minor, 0, i, order);  
        // the recusion is here!
        det += Math::Pow(-1.0, i) * mat[0*order+i] * CalcCustomDeterminant(minor, order-1);
    }  
	
    // release memory
    delete [] minor;
	
    return det;
}

























